% MotionCallBack.m

function MotionCallBack(obj, event)
    % theta14 = [theta1, theta4]
    % disp("MotionCallBack: ");
    % disp(obj.UserData);

    % Control table address
    ADDR_PRO_TORQUE_ENABLE       = 64;         % Control table address is different in Dynamixel model
    ADDR_PRO_GOAL_POSITION       = 116;
    ADDR_PRO_PRESENT_POSITION    = 132;
    
    % Protocol version
    PROTOCOL_VERSION            = 2.0;          % See which protocol version is used in the Dynamixel
    
    load("ConstantDefinition.mat", "DXL_ID", "port_num");
    load("ThetaData.mat", "Angel_theta24");
    theta14 = Angel_theta24(: , obj.UserData);
    obj.UserData = mod(obj.UserData + 1, size(Angel_theta24,2) + 1);
    if obj.UserData == 0
        stop(obj);
    end

    % disp(theta14)
        
    COMM_SUCCESS                = 0;            % Communication Success result value
    COMM_TX_FAIL                = -1001;        % Communication Tx Failed
    
    dxl_comm_result = COMM_TX_FAIL;           % Communication result
    dxl_error = 0;                              % Dynamixel error
    
    % Write goal position
    % theta1
    write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(1), ADDR_PRO_GOAL_POSITION, typecast(int32(theta14(1)), 'uint32'));
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    end
    % theta4
    write4ByteTxRx(port_num, PROTOCOL_VERSION, DXL_ID(2), ADDR_PRO_GOAL_POSITION, typecast(int32(theta14(2)), 'uint32'));
    dxl_comm_result = getLastTxRxResult(port_num, PROTOCOL_VERSION);
    dxl_error = getLastRxPacketError(port_num, PROTOCOL_VERSION);
    if dxl_comm_result ~= COMM_SUCCESS
        fprintf('%s\n', getTxRxResult(PROTOCOL_VERSION, dxl_comm_result));
    elseif dxl_error ~= 0
        fprintf('%s\n', getRxPacketError(PROTOCOL_VERSION, dxl_error));
    end

    % txt1 = ' event occurred at ';
    % 
    % event_type = event.Type;
    % event_time = datestr(event.Data.time);
    % 
    % msg = [event_type txt1 event_time];
    % disp(msg)

% Used others' api
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright 2017 ROBOTIS CO., LTD.
%
% Licensed under the Apache License, Version 2.0 (the "License");
% you may not use this file except in compliance with the License.
% You may obtain a copy of the License at
%
%     http://www.apache.org/licenses/LICENSE-2.0
%
% Unless required by applicable law or agreed to in writing, software
% distributed under the License is distributed on an "AS IS" BASIS,
% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
% See the License for the specific language governing permissions and
% limitations under the License.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%